# -*- coding: utf-8 -*-
"""
Created on Wed Apr  5 13:11:11 2023

@author: ZNZZ
"""
# Make sure to have the server side running in CoppeliaSim: 
# in a child script of a CoppeliaSim scene, add following command
# to be executed just once, at simulation start:
#
# simRemoteApi.start(19999)
#
# then start simulation, and run this program.
#
# IMPORTANT: for each successful call to simxStart, there
# should be a corresponding call to simxFinish at the end!




try:
    import sim
except:
    print ('--------------------------------------------------------------')
    print ('"sim.py" could not be imported. This means very probably that')
    print ('either "sim.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "sim.py"')
    print ('--------------------------------------------------------------')
    print ('')

import time
import math 

class MyArmBasicClass():
    
    def __init__(self):
        print('MyArmTest Program started')
        self.clientID = 0
        self.Handle={  #字典，用于保存各个模块的句柄，方便拓展和查找；key值要和CoppeliaSim中的模块命名一致
            "ShapeBase":0,
            "ShapArm1":0,
            "ShapArm2":0,
            "ShapArm3":0,
            "BaseJoint":0,
            "ArmJoint1":0,
            "ArmJoint2":0,
            }
        self.HandleOrder=(   #元组有固定的顺序，所以用元组作为顺序记录，要和上面的Handle的一致
            "ShapeBase",
            "ShapArm1",
            "ShapArm2",
            "ShapArm3",
            "BaseJoint",
            "ArmJoint1",
            "ArmJoint2",          
            )
        
        
        
        

        
    def ConnectedStart(self):
        '''
        连接初始化。
        需要首先在 CoppeliaSimEdu 中点击运行，再运行本python程序，才能正确连接。
        返回-1表示连接失败

        Returns
        -------
        TYPE
            DESCRIPTION.

        '''
        sim.simxFinish(-1) # just in case, close all opened connections
        self.clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
        if self.clientID!=-1:
            print ('Connected to remote API server')
            return 0
        else:
            print ('Connected faile,please check!')
            return -1
        
    def GetArmHandle(self):
        '''
        获取各个模块的句柄，用于操控
        有些模块的句柄获取不到
        
        '''
        for i in  self.HandleOrder:
            self.Handle[i]  = sim.simxGetObjectHandle(self.clientID, i, sim.simx_opmode_blocking) #获取句柄,返回两个值，一个是ret，用于判断是否获取成功，一个是obj，表示句柄号，两个值以元组的方式存到Handle中
            print(self.Handle[i][0])
            if self.Handle[i][0] != sim.simx_return_ok:
                print("Get "+i+" Handle Error!!") 
            else:
                print("Get "+i+" Handle OK!!") 
                   #实测可以看到，只获取了 底座和三个关节的句柄，所以能操控底座的位置，能操控三个关节的角度，但是不能操作三个臂
                   
    def GetShapeBasePosition(self):
        '''
        获取底座的位置
        
        '''          
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                ret, arr = sim.simxGetObjectPosition(self.clientID, self.Handle[self.HandleOrder[0]][1], -1, sim.simx_opmode_blocking)
                print(ret,arr)
                return ret, arr
        else:    
            print("Something Error!!")
            
                
    def SetShapeBasePosition(self,X,Y,Z):
        '''
        设置底座的位置

        Parameters
        ----------
        (X,Y,Z) : TYPE
            DESCRIPTION:目标坐标（世界坐标系）

        Returns
        -------
        None.

        '''
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                sim.simxSetObjectPosition(self.clientID, self.Handle[self.HandleOrder[0]][1],-1,(X,Y,Z), sim.simx_opmode_blocking)
                print("Set ShapeBase Pos to X:"+str(X)+" Y:"+str(Y)+"  Z:"+str(Z))
        
        else:    
            print("Something Error in SetShapeBasePosition!!")
        
        
        
    
    def GetJointAngle(self,num):
        '''
        获取旋转关节的角度

        Parameters
        ----------
        num : TYPE：控制哪个joint关节
            DESCRIPTION：可以输入 0 1 2  分分别表示 ShapeBase ArmJoint1 ArmJoint2 
                        也可以直接输入字符串 ShapeBase ArmJoint1 ArmJoint2 

        Returns
        -------
        None.

        '''
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                if str(type(num)) == "<class 'int'>":  #先判断输入的num类型
                    if num==1:
                        targetObj_Revolute_joint = "ArmJoint1"
                    elif num == 2:
                        targetObj_Revolute_joint = "ArmJoint2"
                    elif num == 0:
                        targetObj_Revolute_joint = "ShapeBase"
                    else:
                        print("Joint num Error !!")
                        return 
                elif str(type(num)) == "<class 'str'>":
                    targetObj_Revolute_joint = num
                    
                else:
                    print("Joint num type Erroe,Pleace give 1 or 2 or stringName")
                    
                position = sim.simxGetJointPosition(self.clientID, self.Handle[targetObj_Revolute_joint][1], sim.simx_opmode_blocking)
                
                print("Joint "+targetObj_Revolute_joint + " Angle is "+str(position))
                return position
                
        else:    
            print("Something Error in GetJointAngle!!")
        
        
        
    
        
        
    
    def SetJointAngle(self,num,angle):
        '''
        设置关节角度/位置
        对于旋转关节，是设置角度值，内部需要转换为弧度；对于移动关节，我还没搞明白其单位，推测是米，所以mm需要除以1000

        Parameters
        ----------
        num : TYPE：控制哪个joint关节
            DESCRIPTION：可以输入 0 1 2  分分别表示 ShapeBase ArmJoint1 ArmJoint2 
                        也可以直接输入字符串 ShapeBase ArmJoint1 ArmJoint2 
        angle : TYPE  对于  ArmJoint1 ArmJoint2 ，为旋转得到角度值，对于ShapeBase，就是拉伸，暂时没注意具体拉伸多少
            DESCRIPTION.

        Returns
        -------
        None.

        '''
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                if str(type(num)) == "<class 'int'>":  #先判断输入的num类型
                    if num==1:
                        targetObj_Revolute_joint = "ArmJoint1"
                    elif num == 2:
                        targetObj_Revolute_joint = "ArmJoint2"
                    elif num == 0:
                        targetObj_Revolute_joint = "ShapeBase"                        
                        
                    else:
                        print("Joint num Error !!")
                        return -1
                elif str(type(num)) == "<class 'str'>":
                    targetObj_Revolute_joint = num
                    
                else:
                    print("Joint num type Erroe,Pleace give 1 or 2 or stringName")
                    return -1
                    
                    
                if  targetObj_Revolute_joint ==    "ArmJoint1" or  targetObj_Revolute_joint ==    "ArmJoint2":
                    setangle = angle*math.pi/90#角度要转为弧度,但是弧度计算不是 A*π/180 吗，此处90才是正常的？
                else:
                    #ShapeBase 关节不是角度，是运行，目前还没弄清数值与实际运动的关系
                    setangle = angle
                
                sim.simxSetJointPosition(self.clientID, self.Handle[targetObj_Revolute_joint][1], setangle, sim.simx_opmode_blocking) 
                print("Set " + targetObj_Revolute_joint + "Angle to "+str(angle))

               
        else:    
            print("Something Error in GetJointAngle!!")    
        
        
    def ConnectedStop(self):
        '''
        断开操控连接

        Returns
        -------
        None.

        '''
        if self.clientID != -1:
        
            # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
            sim.simxGetPingTime(self.clientID)
    
            sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
    
            # Now close the connection to CoppeliaSim:
            sim.simxFinish(self.clientID)
            
            print("Connecte Stoped!!")
            
        else:
            print("Pleace check clientID !")
        
        
            
  

         
            
def main():
    '''
    简易机械臂控制测试

    '''
    MyArmClass = MyArmBasicClass()
    ret = MyArmClass.ConnectedStart()
    if ret == 0: #连接成功
        MyArmClass.GetArmHandle()
        MyArmClass.GetShapeBasePosition()#获取底座坐标
        position1 = MyArmClass.GetJointAngle("ArmJoint1")  #获取三个关节的角度/位置
        position2 = MyArmClass.GetJointAngle("ArmJoint2")
        position1 = MyArmClass.GetJointAngle("BaseJoint")
        
    
        movedir = 0
        movecount = 0
        movestep = 5    #步进量，可用于调速
        moveangle = 90 #目标角度
        
        #让机械臂循环流畅动起来
        
        while(True):
            if movedir == 0:
               movecount = movecount-movestep
               if movecount < -moveangle*movestep:
                   movedir = 1
                   
            elif movedir == 1:
                movecount = movecount+movestep
                if movecount > moveangle*movestep:
                    movedir = 0
            MyArmClass.SetJointAngle("ArmJoint1",movecount/10)
            MyArmClass.SetJointAngle("ArmJoint2",movecount/10)
            MyArmClass.SetJointAngle("BaseJoint",movecount/5000) #可以看到机械臂在上下运动
        
        
        MyArmClass.ConnectedStop() #若是需要断掉，请主动调用
    else:
        print("Connected Error!! Pleace check and retry!!")
        print("需要先在  coppeliasim 软件中开启仿真，再开始本python才能正确远程连接！！")
                      
            
if __name__ == '__main__':
     #multiprocessing.freeze_support()
    main()
          


    